import java.awt.*;
import java.util.*;
import java.util.List;

import org.apache.commons.math3.analysis.function.Gaussian;

/**
 * Created with IntelliJ IDEA.
 * User: Marcin
 * Date: 04.05.13
 * Time: 22:12
 * To change this template use File | Settings | File Templates.
 */
public class ExampleAgent extends Agent {

    private int selectionSteps = 0;

    public ExampleAgent(Map map) {
        super(map);
    }


    @Override
    public void move(Direction direction, double rotation, double moveNoiseParam, double laserNoiseParam, int sigma, int laserSize) {
        //TODO dodac niepenowsc lasera laserNoiseParam -> zakres wartosci <0.01 - 1>
        Random r = new Random();
        for (Particle particle : particles) {
            particle.moveWithNoise(direction, map, moveNoiseParam);
            double weight = 1;
            List<Point> laserPoints = senseForParticle(particle.getX(),particle.getY(), laserSize);
            int i=0;
            for (Point orientationPoint : laserShoots) {

                double distanceFromRobo = orientationPoint.distance(map.getAgentPositionX(), map.getAgentPositionY());
                double param = 50 * laserNoiseParam;
                distanceFromRobo += r.nextGaussian() * param;
                //double distanceFromParticle = orientationPoint.distance(particle.getX(), particle.getY());
                double distanceFromParticle = laserPoints.get(i).distance(particle.getX(),particle.getY());
                double measurementNoise = sigma;

                Gaussian g = new Gaussian(distanceFromRobo, measurementNoise);
                weight *= g.value(distanceFromParticle);
                i++;
            }
            particle.setWeight(weight);
        }

        normalization();
        wheelSelection();
        if (selectionSteps > 0) {
            System.out.println("Wheel Selection");
            //wheelSelection();
            selectionSteps = 0;
        } else {
            selectionSteps++;
        }

    }


    public void normalization() {
        double sum = 0;
        for (Particle particle : particles) {
            sum += particle.getWeight();
        }
        double sumPrint = 0;
        for (Particle particle : particles) {
            particle.setWeight(particle.getWeight() / sum);
            sumPrint += particle.getWeight();
        }
    }

    public void wheelSelection() {
        //liczba czasteczek
        int n = this.getNumberOfParticles();
        //selekcja ruletkowa
        //Sortowanie jest konieczne do wziecia maxa w selekcji
        Collections.sort(particles);
        //for (int i = 0; i < 10; i++) {
        //    System.out.println("Czasteczka nr " + ", [X: " + particles.get(i).getX() + "\tY: " + particles.get(i).getY() + "], waga: " + particles.get(i).getWeight());
        //}
        //System.out.println("-------------");
        List<Particle> resampledList = new ArrayList<Particle>();
        Random r = new Random();
        int idx = r.nextInt(n);
        double b = 0;
        Particle mx = particles.get(0);
        for (int i = 0; i < n; i++) {
            b += 2 * mx.getWeight() * r.nextDouble();
            while (b > particles.get(idx).getWeight()) {
                b -= particles.get(idx).getWeight();
                idx = (idx + 1) % n;
            }
            resampledList.add(particles.get(idx).getCopyOfMe());
        }
        //Collections.sort(list);
        //for (int i = 0; i < 50; i++) {
        //    System.out.println("Czasteczka nr " + ", [X: " + resampledList.get(i).getX() + "\tY: " + resampledList.get(i).getY() + "], waga: " + resampledList.get(i).getWeight());
        //}
        particles = new ArrayList<Particle>();
        particles.addAll(resampledList);
        //normalization();

    }

    @Override
    public void sense(int laserSize) {
        laserShoots.clear();
        for (double i = 0; i < 2 * Math.PI; i = i + (2 * Math.PI / laserSize)) {
            Point orientationPoint = new Point(map.getPointOfNearestObject(map.getAgentPositionX(), map.getAgentPositionY(), i + getRotation()));
            laserShoots.add(orientationPoint);
        }
    }

    public List<Point> senseForParticle(int x, int y, int laserSize) {

        List<Point> lasersPoints =  new ArrayList<Point>();
        for (double i = 0; i < 2 * Math.PI; i = i + (2 * Math.PI / laserSize)) {
            Point orientationPoint = new Point(map.getPointOfNearestObject(x, y, i + getRotation()));
            lasersPoints.add(orientationPoint);
        }
        return lasersPoints;
    }

}
